#include "Quaternion.h"
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

Quaternion::Quaternion(void)
{
	w = 1.0f;
	x = 0.0f;
	y = 0.0f;
	z = 0.0f;
}
Quaternion::Quaternion(Vector *vector)
{
	w = 0.0f;
	x = (GLfloat)vector->thisVector.x;
	y = (GLfloat)vector->thisVector.y;
	z = (GLfloat)vector->thisVector.z;
}
Quaternion::Quaternion(Vector vector)
{
	w = 0.0f;
	x = (GLfloat)vector.thisVector.x;
	y = (GLfloat)vector.thisVector.y;
	z = (GLfloat)vector.thisVector.z;
}
Quaternion::Quaternion(float angleDegrees, bool _x, bool _y, bool _z)
{
	float radians = (angleDegrees * M_PI)/180;
	radians = radians/2;
	
	if(_x)
	{
		w = (GLfloat)cos(radians);
		x = (GLfloat)sin(radians);
		y = 0.0f;
		z = 0.0f;
	}
	else if(_y)
	{
		w = (GLfloat)cos(radians);
		x = 0.0f;
		y = (GLfloat)sin(radians);
		z = 0.0f;
	}
	else if(_z)
	{
		w = (GLfloat)cos(radians);
		x = 0.0f;
		y = 0.0f;
		z =	(GLfloat)sin(radians);
	}
}

Quaternion::Quaternion(float angleDegrees, Vector *vector)
{
	float radians = (angleDegrees * M_PI)/180;
	radians = radians/2;
	w = (GLfloat)cos(radians);
	x = (GLfloat)(vector->thisVector.x * sin(radians));
	y = (GLfloat)(vector->thisVector.y * sin(radians));
	z = (GLfloat)(vector->thisVector.z * sin(radians));
}
Quaternion::Quaternion(float angleDegrees, Vector vector)
{
	float radians = (angleDegrees * M_PI)/180;
	radians = radians/2;
	w = (GLfloat)cos(radians);
	x = (GLfloat)(vector.thisVector.x * sin(radians));
	y = (GLfloat)(vector.thisVector.y * sin(radians));
	z = (GLfloat)(vector.thisVector.z * sin(radians));
}

Quaternion::Quaternion(float _w, float _x, float _y, float _z)
{
	w = (GLfloat)_w;
	x = (GLfloat)_x;
	y = (GLfloat)_y;
	z = (GLfloat)_z;
}

Quaternion Quaternion::multiply(Quaternion *other)
{
	Quaternion result;
	result.w = (this->w * other->w) - (this->x * other->x) - (this->y * other->y) - (this->z * other->z);
	result.x = (this->w * other->x) + (this->x * other->w) + (this->y * other->z) - (this->z * other->y);
	result.y = (this->w * other->y) - (this->x * other->z) + (this->y * other->w) + (this->z * other->x);
	result.z = (this->w * other->z) + (this->x * other->y) - (this->y * other->x) + (this->z * other->w);
	return result;
}

Quaternion Quaternion::multiply(Quaternion other)
{
	Quaternion result;
	result.w = (this->w * other.w) - (this->x * other.x) - (this->y * other.y) - (this->z * other.z);
	result.x = (this->w * other.x) + (this->x * other.w) + (this->y * other.z) - (this->z * other.y);
	result.y = (this->w * other.y) - (this->x * other.z) + (this->y * other.w) + (this->z * other.x);
	result.z = (this->w * other.z) + (this->x * other.y) - (this->y * other.x) + (this->z * other.w);
	return result;
}

Quaternion Quaternion::add(Quaternion *other)
{
	Quaternion result;
	result.w = this->w + other->w;
	result.x = this->x + other->x;
	result.y = this->y + other->y;
	result.z = this->z + other->z;

	return result;
}

Quaternion Quaternion::add(Quaternion other)
{
	Quaternion result;
	result.w = this->w + other.w;
	result.x = this->x + other.x;
	result.y = this->y + other.y;
	result.z = this->z + other.z;

	return result;
}

Quaternion Quaternion::subtract(Quaternion *other)
{
	Quaternion result;
	result.w = this->w - other->w;
	result.x = this->x - other->x;
	result.y = this->y - other->y;
	result.z = this->z - other->z;

	return result;
}

Quaternion Quaternion::subtract(Quaternion other)
{
	Quaternion result;
	result.w = this->w - other.w;
	result.x = this->x - other.x;
	result.y = this->y - other.y;
	result.z = this->z - other.z;

	return result;
}

GLfloat Quaternion::dotProduct(Quaternion *other)
{
	return( (this->w*other->w) + (this->x*other->x) + (this->y*other->y) + (this->z*other->z) );
}

GLfloat Quaternion::dotProduct(Quaternion other)
{
	return( (this->w*other.w) + (this->x*other.x) + (this->y*other.y) + (this->z*other.z) );
}

GLfloat Quaternion::returnLength()
{
	return sqrt(pow(w,2) + pow(x,2) + pow(y,2) + pow(z,2));
}

void Quaternion::normalize()
{
	GLfloat div = this->returnLength();
	w = w/div;
	x = x/div;
	y = y/div;
	z = z/div;
}

Quaternion Quaternion::conjugate()
{
	return Quaternion(this->w, (-1) * this->x, (-1) * this->y, (-1) * this->z);
}


Quaternion::~Quaternion(void)
{
}
